#include <chrono>
#include "cartographer_ros_msgs/msg/status_response.hpp"
#include "cartographer_ros_msgs/srv/write_state.hpp"
#include "cartographer_ros_msgs/srv/finish_trajectory.hpp"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

DEFINE_int32(trajectory_id, 0, "trajectory_id");
DEFINE_bool(include_unfinished_submaps, false, "");
DEFINE_string(logfile, "", "logfile");
DEFINE_string(pbstream_filename, "", "Filename of a pbstream to draw a map from.");

#define APP_NAME "save_map"

class MapClient
{
public:
    MapClient();

    void CallBack();
    int FinishTrajectory(int traj_id);
    int WriteState(std::string filename, bool include_unfinished_submaps);

    bool stop = false;
    int res_finish_traj_ = 0;
    int res_write_state_ = 0;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<rclcpp::Node> node_;
};

MapClient::MapClient()
{
    node_ = rclcpp::Node::make_shared(APP_NAME);
    timer_ = node_->create_wall_timer(1s, std::bind(&MapClient::CallBack, this));
}

void MapClient::CallBack()
{
    if (res_finish_traj_ > 0 && res_write_state_ < 1)
    {
        res_write_state_ = WriteState(FLAGS_pbstream_filename, FLAGS_include_unfinished_submaps);
        std::cout << "WriteState res: " << res_write_state_ << "\n";
        if (res_write_state_ > 0)
        {
            timer_.reset();
        }
    }

    if (res_finish_traj_ < 1)
    {
        res_finish_traj_ = FinishTrajectory(FLAGS_trajectory_id);
        std::cout << "FinishTrajectory res: " << res_finish_traj_ << "\n";
    }
}

int MapClient::WriteState(std::string filename, bool include_unfinished_submaps)
{
    std::cout << "WriteState\n";
    auto node = rclcpp::Node::make_shared("client_state");
    auto client = node->create_client<cartographer_ros_msgs::srv::WriteState>("/write_state");
    auto request = std::make_shared<cartographer_ros_msgs::srv::WriteState::Request>();
    request->filename = filename;
    request->include_unfinished_submaps = include_unfinished_submaps;
    while (!client->wait_for_service(1s))
    {
        if (!rclcpp::ok())
        {
            std::cout << "Interrupted while waiting for service\n";
            return -1;
        }
        std::cout << "service not available, waiting again...\n";
    }
    auto result = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS)
    {
        std::cout << "Failed to call service\n";
        return -2;
    }
    auto resp = result.get();
    if (resp)
    {
        auto code = resp->status.code;
        auto message = resp->status.message;
        std::cout << "service res: " << code << message << "\n";
        return 1;
    }
    return 2;
}

int MapClient::FinishTrajectory(int traj_id)
{
    std::cout << "FinishTrajectory\n";
    auto node = rclcpp::Node::make_shared("client_trajectory");
    auto client = node->create_client<cartographer_ros_msgs::srv::FinishTrajectory>("/finish_trajectory");
    auto request = std::make_shared<cartographer_ros_msgs::srv::FinishTrajectory::Request>();
    request->trajectory_id = traj_id;
    while (!client->wait_for_service(1s))
    {
        if (!rclcpp::ok())
        {
            std::cout << "Interrupted while waiting for service\n";
            return -1;
        }
        std::cout << "service not available, waiting again...\n";
    }
    auto result = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS)
    {
        std::cout << "Failed to call service\n";
        return -2;
    }
    auto resp = result.get();
    if (resp)
    {
        auto code = resp->status.code;
        auto message = resp->status.message;
        std::cout << "service res: " << code << message << "\n";
        return 1;
    }
    return 2;
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    google::AllowCommandLineReparsing();
    FLAGS_alsologtostderr = true;
    google::InitGoogleLogging(argv[0]);
    if (!FLAGS_logfile.empty())
        google::SetLogDestination(google::INFO, FLAGS_logfile.c_str());
    google::ParseCommandLineFlags(&argc, &argv, false);
    CHECK(!FLAGS_pbstream_filename.empty()) << "pbstream_filename is missing.";

    MapClient client;
    rclcpp::spin(client.node_);
    // rclcpp::executors::SingleThreadedExecutor exec;
    //  exec.add_node(client.node_);
    //  exec.add_node(client.node_state_);
    //  exec.add_node(client.node_trajectory_);
    //  exec.spin();
    rclcpp::shutdown();
}
